package osl.ev3;

import lejos.hardware.lcd.LCD;
import lejos.hardware.motor.Motor;
import lejos.hardware.port.SensorPort;
import lejos.hardware.sensor.EV3UltrasonicSensor;
import lejos.robotics.RegulatedMotor;
import lejos.robotics.SampleProvider;
import lejos.robotics.subsumption.Arbitrator;
import lejos.robotics.subsumption.Behavior;

public class BumperCar {

	 static RegulatedMotor leftMotor = Motor.B;
	 static RegulatedMotor rightMotor = Motor.C;
	 static ultrasonicSensor sensor;
	    
	  public static void main(String[] args)
	  {
		  
		    leftMotor.resetTachoCount();
		    rightMotor.resetTachoCount();
		    leftMotor.rotateTo(0);
		    rightMotor.rotateTo(0);
		    leftMotor.setSpeed(400);
		    rightMotor.setSpeed(400);
		    leftMotor.setAcceleration(800);
		    rightMotor.setAcceleration(800);
		    sensor = new ultrasonicSensor();
		    sensor.setDaemon(true);
		    sensor.start();
		    Behavior b1 = new DriveForward();
		    Behavior b2 = new DetectWall();
		    Behavior[] behaviorList =
		    {
		      b1, b2
		    };
		    Arbitrator arbitrator = new Arbitrator(behaviorList);
		    LCD.drawString("Bumper Car",0,1);
		    arbitrator.start();
	  }
	}

	class ultrasonicSensor extends Thread
	{
	    EV3UltrasonicSensor us = new EV3UltrasonicSensor(SensorPort.S4);
	    public int distance = 255;

	    ultrasonicSensor()
	    {

	    }
	    
	    public void run()
	    {
	    	us.enable();
	    	SampleProvider sampleProvider = us.getDistanceMode();
	        while (true)
	        {
	            float [] sample = new float[sampleProvider.sampleSize()];
	            sampleProvider.fetchSample(sample, 0);
	            distance = (int)sample[0];
	            System.out.println("Distance: " + distance);
	            
	        }
	        
	    }
	}

	class DriveForward implements Behavior
	{

	  private boolean _suppressed = false;

	  public boolean takeControl()
	  {
	    return true; 
	  }

	  public void suppress()
	  {
	    _suppressed = true;
	  }

	  public void action()
	  {
	    _suppressed = false;
	    while (!_suppressed)
	    {
	      BumperCar.leftMotor.forward();
	      BumperCar.rightMotor.forward();
	       
	      Thread.yield(); 
	    }
	  }
	}


	class DetectWall implements Behavior
	{

	  public DetectWall()
	  {
	  }
	  
	  
	  private boolean checkDistance()
	  {

	      int dist = BumperCar.sensor.distance;
	      if (dist < 30)
	      {
	          return true;
	      }
	      else
	      {
	          return false;
	      }
	  }
	  
	  public boolean takeControl()
	  {
	    return checkDistance();
	  }

	  public void suppress()
	  {
	    
	  }

	  public void action()
	  {
	      BumperCar.leftMotor.rotate(-180, true);
	      BumperCar.rightMotor.rotate(-180);  
	    if ((System.currentTimeMillis() & 0x1) != 0)
	    {
	        BumperCar.leftMotor.rotate(-180, true);
	        BumperCar.rightMotor.rotate(180);  
	    }
	    else
	    {
	        BumperCar.rightMotor.rotate(-180, true);
	        BumperCar.leftMotor.rotate(180);          
	    }
	  }
	  
	}



